{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "%pylab inline\n",
    "from sympy import init_printing,Matrix\n",
    "init_printing(use_latex='mathjax')\n",
    "from scipy import weave\n",
    "import os\n",
    "import cv2\n",
    "import pickle\n",
    "from robot import transl,r2t,t2r,rpy2tr"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 相机建模"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 相机参数\n",
    "class Cam(object):\n",
    "    def __init__(self,name=\"0\"):\n",
    "        self.name = name\n",
    "        self.unit = 20.0 # 标定板单位为 20mm\n",
    "        self.height = 480 # 相机标定时的高\n",
    "        self.width = 640 # 相机标定时的宽\n",
    "        self.mtx = self.get_cameraMatrix()\n",
    "        self.dist = self.get_distCoeffs()\n",
    "        self.gen_new_image()\n",
    "        self.Tr = mat(eye(4)) # 世界坐标系在相机下的表示 Tc_o \n",
    "\n",
    "    def get_cameraMatrix(self):\n",
    "        mtx = np.eye(3)\n",
    "        fx = 1511.64442682\n",
    "        fy = 1514.72927917\n",
    "        cx = 331.982535978\n",
    "        cy = 268.613047699\n",
    "        mtx[0,0],mtx[1,1],mtx[0,2],mtx[1,2] = fx,fy,cx,cy\n",
    "        return mtx\n",
    "\n",
    "    def get_distCoeffs(self):\n",
    "        dist = np.zeros((1,5))\n",
    "        k1 = -0.105356293294\n",
    "        k2 = 3.25067090251\n",
    "        p1 = 0.00155905212496\n",
    "        p2 = -0.00569751368371\n",
    "        k3 = -25.6347944753\n",
    "        dist[0] = k1,k2,p1,p2,k3\n",
    "        return dist\n",
    "\n",
    "    def gen_new_image(self):\n",
    "        self.image = 255+np.zeros((self.height,self.width,3),dtype=np.uint8)\n",
    "        \n",
    "    def get_image(self):\n",
    "        return self.image\n",
    "    \n",
    "    def show_image(self):\n",
    "        imshow(self.image)\n",
    "        \n",
    "    def plot_point(self,p3d,color=(255,0,0),show=True):\n",
    "        p = np.vstack((p3d,1))\n",
    "        cp = self.Tr*p\n",
    "        #print \"cp:\",cp\n",
    "        imgPoint = cv2.projectPoints(p[:3,0].reshape((1,3,1)),t2r(self.Tr),transl(self.Tr),self.mtx,self.dist)[0] # 投影到相机\n",
    "        imgPoint = imgPoint[0,0]\n",
    "        p2d = mat(imgPoint).T\n",
    "        #print \"p2d:\", p2d\n",
    "        if show:\n",
    "            self.plot_img_point(p2d,color)\n",
    "        return p2d\n",
    "        \n",
    "    def plot_img_point(self,p2d,color=(255,0,0)):\n",
    "        x,y = array(p2d).flatten()\n",
    "        x = int(x)\n",
    "        y = int(y)\n",
    "        cv2.circle(self.image, (x,y), 2, color,2)\n",
    "    \n",
    "    def plot_img_line(self,p2d0,p2d1,color=(255,0,0)):\n",
    "        a,b = array(p2d0).flatten()\n",
    "        c,d = array(p2d1).flatten()\n",
    "        cv2.line(self.image,(int(a),int(b)),(int(c),int(d)),color,1)\n",
    "        \n",
    "    def plot_robot_point(self,robot,q0,color=(255,0,0)):\n",
    "        p = robot.fkine(q0)[:3,3]\n",
    "        return self.plot_point(p,color)\n",
    "    \n",
    "    def get_jacobn(self,p2d,p3d,rot=True):\n",
    "        '''\n",
    "        u: 图像的x坐标\n",
    "        v: 图像的y坐标\n",
    "        z: 目标点在图像坐标系下当前的z坐标值\n",
    "        rot: 是否考虑点的空间转动,若不考虑则仅取前3列,即仅考虑点的移动\n",
    "        '''\n",
    "        u,v,z = p2d[0,0], p2d[1,0], p3d[2,0]\n",
    "        # 在图像坐标系下三维空间的图像雅克比\n",
    "        mtx = self.mtx\n",
    "        fx,fy,cx,cy = mtx[0,0],mtx[1,1],mtx[0,2],mtx[1,2]\n",
    "        if rot:\n",
    "            jacobn = np.zeros((2,6),dtype=np.float64)\n",
    "        else:\n",
    "            jacobn = np.zeros((2,3),dtype=np.float64)\n",
    "            \n",
    "        jacobn[0,0] = fx/z\n",
    "        jacobn[0,2] = -u/z\n",
    "        jacobn[1,1] = fy/z\n",
    "        jacobn[1,2] = -v/z\n",
    "\n",
    "        if rot:\n",
    "            jacobn[0,3] = -u*v/fy\n",
    "            jacobn[0,4] = (fx**2 + u**2)/fx\n",
    "            jacobn[0,5] = -fx*v/fy\n",
    "            jacobn[1,3] = -(fy**2+v**2)/fy\n",
    "            jacobn[1,4] = u*v/fx\n",
    "            jacobn[1,5] = fy*u/fx\n",
    "            \n",
    "        return mat(jacobn)\n",
    "        \n",
    "    def get_jacob0(self,p2d,p3d,rot=True):\n",
    "        '''\n",
    "        u: 图像的x坐标\n",
    "        v: 图像的y坐标\n",
    "        x: 目标点在世界坐标系下当前的x坐标值\n",
    "        y: 目标点在世界坐标系下当前的y坐标值\n",
    "        z: 目标点在世界坐标系下当前的z坐标值\n",
    "        '''\n",
    "        p = np.vstack((p3d,1))\n",
    "        cp = self.Tr*p\n",
    "        jacobn = self.get_jacobn(p2d,cp,rot)\n",
    "        R = t2r(self.Tr)\n",
    "        if rot:\n",
    "            jacob_n_o = eye(6)\n",
    "            jacob_n_o[:3,:3] = R\n",
    "            jacob_n_o[3:,3:] = R\n",
    "        else:\n",
    "            jacob_n_o = R\n",
    "        jcaob0 = jacobn*jacob_n_o\n",
    "        return jcaob0\n",
    "    \n",
    "    def get_jacobs(self,p2d,robot,q0,rot=True):\n",
    "        # 考虑全部的图像雅克比(移动和转动)\n",
    "        jacob0 = robot.jacob0(q0)\n",
    "        p3d = robot.fkine(q0)[:3,3]\n",
    "        if not rot:\n",
    "            jacob0 = jacob0[:3,:]\n",
    "        jacob_cs = self.get_jacob0(p2d,p3d,rot)\n",
    "        return jacob_cs*jacob0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def show_cams(cams):\n",
    "    for cam in cams:\n",
    "        plt.figure()\n",
    "        plt.imshow(cam.image)\n",
    "        plt.title(cam.name)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def clear_cams(cams):\n",
    "    for cam in cams:\n",
    "        cam.gen_new_image()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def move_point(p3d,v3d,w3d):\n",
    "    x,y,z = array(p3d).flatten()\n",
    "    wx,wy,wz = array(w3d).flatten()\n",
    "    out_p3d = p3d.copy()\n",
    "    out_p3d += v3d\n",
    "    out_p3d[0,0] += z*wy - y*wz\n",
    "    out_p3d[1,0] += x*wz - z*wx\n",
    "    out_p3d[2,0] += y*wx - x*wy\n",
    "    return out_p3d"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 图像雅克比的解析解释"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 求图像雅克比( Tr = I )"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "cam = Cam(\"Tr = E\")\n",
    "p3d = mat([-10.0,-15.0,500.0]).T # 空间点坐标\n",
    "v3d = mat([0.1,0.2,0.4]).T # 空间速度，因为很小，所以在图像上应该没有变化\n",
    "w3d = mat([0.006,0.01,0.003]).T # 空间角速度\n",
    "p2d = cam.plot_point(p3d,(0,0,0)) # p3d 在图像中的图像点\n",
    "print p2d-cam.plot_point(p3d+v3d,show=False) # 不考虑转动时，图像上点的变化应该很小\n",
    "new_p3d = move_point(p3d,v3d,w3d) # 移动后的实际 3d 坐标\n",
    "real_new_p2d = cam.plot_point(new_p3d,(0,255,0)) # 移动后实际 3d 点映射到图像上的图像点\n",
    "show_cams([cam])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "jacob0 = cam.get_jacob0(p2d,p3d) # 图像雅克比矩阵\n",
    "delta_img_point = jacob0*np.vstack((v3d,w3d)) # 使用图像雅克比计算得到的图像点变化向量\n",
    "new_p2d = p2d + delta_img_point # 使用图像雅克比计算得到的新图像点\n",
    "cam.plot_img_point(new_p2d,(255,0,0)) # 实际计算得到的图像点\n",
    "print real_new_p2d - new_p2d # 计算得到的与实际的差别\n",
    "show_cams([cam])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": false
   },
   "source": [
    "### 求图像雅克比( Tr != I )"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "cam = Cam(\"Tr != E\")\n",
    "cam.Tr = rpy2tr(0.01,0.02,0.03)*transl(30,20,30)\n",
    "p3d = mat([-10.0,-15.0,500.0]).T # 空间点坐标\n",
    "v3d = mat([0.1,0.2,0.4]).T # 空间速度，因为很小，所以在图像上应该没有变化\n",
    "w3d = mat([0.006,0.01,0.003]).T # 空间角速度\n",
    "p2d = cam.plot_point(p3d,(0,0,0)) # p3d 在图像中的图像点\n",
    "new_p3d = move_point(p3d,v3d,w3d) # 移动后的实际 3d 坐标\n",
    "real_new_p2d = cam.plot_point(new_p3d,(0,255,0)) # 移动后实际 3d 点映射到图像上的图像点\n",
    "show_cams([cam])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "jacob0 = cam.get_jacob0(p2d,p3d) # 图像雅克比矩阵\n",
    "delta_img_point = jacob0*np.vstack((v3d,w3d)) # 使用图像雅克比计算得到的图像点变化向量\n",
    "new_p2d = p2d + delta_img_point # 使用图像雅克比计算得到的新图像点\n",
    "cam.plot_img_point(new_p2d,(255,0,0)) # 实际计算得到的图像点\n",
    "print real_new_p2d - new_p2d # 计算得到的与实际的差别\n",
    "show_cams([cam])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 加大角速度\n",
    "\n",
    "会导致结果变差"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "cam = Cam(\"biger w3d\")\n",
    "cam.Tr = rpy2tr(0.01,0.02,0.03)*transl(30,20,30)\n",
    "p3d = mat([-10.0,-15.0,500.0]).T # 空间点坐标\n",
    "v3d = mat([0.01,0.02,0.04]).T\n",
    "w3d = mat([0.01,0.02,0.03]).T\n",
    "p2d = cam.plot_point(p3d,(0,0,0)) # p3d 在图像中的图像点\n",
    "new_p3d = move_point(p3d,v3d,w3d) # 移动后的实际 3d 坐标\n",
    "real_new_p2d = cam.plot_point(new_p3d,(0,255,0)) # 移动后实际 3d 点映射到图像上的图像点\n",
    "show_cams([cam])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "jacob0 = cam.get_jacob0(p2d,p3d) # 图像雅克比矩阵\n",
    "delta_img_point = jacob0*np.vstack((v3d,w3d)) # 使用图像雅克比计算得到的图像点变化向量\n",
    "new_p2d = p2d + delta_img_point # 使用图像雅克比计算得到的新图像点\n",
    "cam.plot_img_point(new_p2d,(255,0,0)) # 实际计算得到的图像点\n",
    "print real_new_p2d - new_p2d # 计算得到的与实际的差别\n",
    "show_cams([cam])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "## 机器人的图像雅克比\n",
    "\n",
    "\n",
    "\\begin{align*}\n",
    "\\begin{bmatrix}\n",
    "\\dot{\\mathrm{u} }\\\\ \n",
    "\\dot{\\mathrm{v} }\n",
    "\\end{bmatrix} &= {}^c J_s \\begin{bmatrix}\n",
    "{}^c \\dot{v}\\\\ \n",
    "{}^c \\dot{\\omega}\n",
    "\\end{bmatrix} \\\\\n",
    "& = {}^c J_s \\begin{bmatrix}\n",
    "{}^c_0 R & 0\\\\ \n",
    "0 & {}^c_0 R\n",
    "\\end{bmatrix} \\begin{bmatrix}\n",
    "{}^0 \\dot{v}\\\\ \n",
    "{}^0 \\dot{\\omega}\n",
    "\\end{bmatrix} \\\\\n",
    "& = {}^c J_s \\begin{bmatrix}\n",
    "{}^c_0 R & 0\\\\ \n",
    "0 & {}^c_0 R\n",
    "\\end{bmatrix} J_0(q) \\dot{q} \\\\\n",
    "& = {}^c J_s \\begin{bmatrix}\n",
    "{}^c_0 R & 0\\\\ \n",
    "0 & {}^c_0 R\n",
    "\\end{bmatrix} \\begin{bmatrix}\n",
    "{}^0_n R & 0\\\\ \n",
    "0 & {}^0_n R\n",
    "\\end{bmatrix} J_n(q) \\dot{q} \\\\\n",
    "\\end{align*}\n",
    "\n",
    "\n",
    "令: \n",
    "\n",
    "$$\n",
    "J_s(q) = {}^c J_s \\begin{bmatrix}\n",
    "{}^c_0 R & 0\\\\ \n",
    "0 & {}^c_0 R\n",
    "\\end{bmatrix} \\begin{bmatrix}\n",
    "{}^0_n R & 0\\\\ \n",
    "0 & {}^0_n R\n",
    "\\end{bmatrix} J_n(q)\n",
    "$$\n",
    "\n",
    "则有:\n",
    "\n",
    "$$\n",
    "\\begin{bmatrix}\n",
    "\\dot{\\mathrm{u} }\\\\ \n",
    "\\dot{\\mathrm{v} }\n",
    "\\end{bmatrix} = J_s(q) \\dot{q}\n",
    "$$\n",
    "\n",
    "$J_s(q)$ 即机器人的图像雅克比, 若有两个图像点位于两个相机中,则有:\n",
    "\n",
    "$$\n",
    "\\begin{bmatrix}\n",
    "\\dot{\\mathrm{u}_1 }\\\\ \n",
    "\\dot{\\mathrm{v}_1 } \\\\\n",
    "\\dot{\\mathrm{u}_2 }\\\\ \n",
    "\\dot{\\mathrm{v}_2 }\n",
    "\\end{bmatrix} = \\begin{bmatrix}\n",
    "J_{s1}(q)\\\\\n",
    "J_{s2}(q)\n",
    "\\end{bmatrix} \\dot{q}\n",
    "$$\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "from mdh_nao import Nao\n",
    "nao = Nao(\"nao\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "cam = Cam(\"one image\")\n",
    "cam.Tr = rpy2tr(0.01,0.02,0.03)*transl(-220,-200,500)\n",
    "\n",
    "q0 = array([0.1,0.2,0.3,0.4,0.5])\n",
    "p3d = nao.LeftHand.fkine(q0)[:3,3] # 初始末端位置\n",
    "p2d = cam.plot_robot_point(nao.LeftHand,q0,(0,0,0)) # p3d 在图像中的图像点\n",
    "delta_q = mat([0.01,0.02,0.003,0.004,0.005]) # 关节空间变化量\n",
    "new_p3d = nao.LeftHand.fkine(q0+delta_q)[:3,3] # 移动后的实际 3d 坐标\n",
    "endPoint = cam.plot_point(new_p3d,(0,255,0)) # 移动后实际 3d 点映射到图像上的图像点\n",
    "show_cams([cam])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 不考虑点的转动\n",
    "\n",
    "不考虑点的转动,仅考虑移动,即仅取图像雅克比的 前3列, 机器人雅克比的前3行"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "cam.gen_new_image()\n",
    "cam.plot_robot_point(nao.LeftHand,q0,(0,0,0)) # 起始点为黑色\n",
    "cam.plot_point(new_p3d,(0,255,0)) # 移动后实际 3d 点映射到图像上的图像点，为绿色\n",
    "img_jacob = cam.get_jacobs(p2d,nao.LeftHand,q0,rot=False)\n",
    "delta_img_point = img_jacob*delta_q.T # 使用图像雅克比计算得到的图像点变化向量\n",
    "new_p2d = p2d + delta_img_point # 使用图像雅克比计算得到的新图像点\n",
    "cam.plot_img_point(new_p2d,(255,0,0)) # 计算得到的下一刻图像点为红色\n",
    "print endPoint - new_p2d # 计算得到的与实际的差别\n",
    "show_cams([cam])  # 误差很小"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "clear_cams([cam])\n",
    "cam.plot_robot_point(nao.LeftHand,q0,(0,0,0)) # 起始点为黑色\n",
    "cam.plot_point(new_p3d,(0,255,0)) # 移动后实际 3d 点映射到图像上的图像点，为绿色\n",
    "img_jacob = cam.get_jacobs(p2d,nao.LeftHand,q0,rot=True)\n",
    "delta_img_point = img_jacob*delta_q.T # 使用图像雅克比计算得到的图像点变化向量\n",
    "new_p2d = p2d + delta_img_point # 使用图像雅克比计算得到的新图像点\n",
    "cam.plot_img_point(new_p2d,(255,0,0)) # 计算得到的下一刻图像点为红色\n",
    "print endPoint - new_p2d # 计算得到的与实际的差别\n",
    "show_cams([cam])\n",
    "# 误差很大,猜想是因为对于空间一点不存在旋转速度，对于刚体上的一点才会存在转动,\n",
    "# 如果一个刚体上有三个点,或许就能用的上,始终不理解"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "new_p3d - p3d # 平移运动"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "nao.LeftHand.jacob0(q0)*delta_q.T # 显然包括平移和转动"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 使用估计雅克比\n",
    "\n",
    "一幅图像,两个约束,一个机械臂5个自由度, 冗余3个自由度,\n",
    "\n",
    "当距离小时,可得到解,但考虑到识别的精度,其实没有实际意义,当与目标距离较大时,发散,解不出"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_end_point(cams,robot,q,color=(255,0,0)):\n",
    "    end_point = None\n",
    "    for cam in cams:\n",
    "        if end_point is None:\n",
    "            end_point = cam.plot_robot_point(robot,q.T,color)\n",
    "        else:\n",
    "            end_point = np.vstack((end_point,cam.plot_robot_point(robot,q.T,color)))\n",
    "    return end_point"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def plot_end_point_line(cams,end_point0,end_point1):\n",
    "    k = 0\n",
    "    for cam in cams:\n",
    "        cam.plot_img_line(end_point0[2*k:2*k+2,0],end_point1[2*k:2*k+2,0])\n",
    "        k = k+1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_jacobs_from_cams(cams,end_point,robot,q,rot=False):\n",
    "    J = None\n",
    "    k = 0\n",
    "    for cam in cams:\n",
    "        if J is None:\n",
    "            J = cam.get_jacobs(end_point[k:k+2,0],robot,q0.T,rot)\n",
    "        else:\n",
    "            J = np.vstack((J,cam.get_jacobs(end_point[k:k+2,0],robot,q0.T,rot)))\n",
    "        k = k+1\n",
    "    return J"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "def be_in_good(q):\n",
    "    # 截断q, 使得q在 -pi,pi\n",
    "    for i,v in enumerate(q):\n",
    "        if v < -pi or v > pi:\n",
    "            v = v - int(v/(2*pi))*2*pi \n",
    "        if v < - pi:\n",
    "            v = v + 2*pi\n",
    "        if v > pi:\n",
    "            v= v - 2*pi\n",
    "        q[i] = v\n",
    "    return q"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_Jk(Jk_1,delta_q,delta_f):\n",
    "    if abs(delta_q.T*delta_q) > 0.01:\n",
    "        Jk = Jk_1+(delta_f - Jk_1*delta_q)*delta_q.T/(delta_q.T*delta_q)\n",
    "    else:\n",
    "        Jk = Jk_1\n",
    "    return Jk"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_next_q(Jk,Jk_1,qk,fk,delta_q):\n",
    "    try:\n",
    "        if abs(delta_q.T*delta_q) > 0.01:\n",
    "            next_q = qk - inv(Jk.T*Jk+((Jk.T-Jk_1.T)*fk*delta_q.T)/(delta_q.T*delta_q))*(Jk.T*fk+(Jk.T-Jk_1.T)*fk)\n",
    "        else:\n",
    "            next_q = qk - inv(Jk.T*Jk)*(Jk.T*fk+(Jk.T-Jk_1.T)*fk)\n",
    "    except:\n",
    "        next_q = qk - pinv(Jk)*fk\n",
    "    return be_in_good(next_q)\n",
    " "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "def move_to(cams,robot,q0,img_point,idea=False,rot=False,use_pinv=False, debug=False,J0=None,err=1.0):\n",
    "    # 将机器人的末端点图像移动到 img_point, 两个相机\n",
    "    end_point = get_end_point(cams,robot,q0)\n",
    "    fk = end_point - img_point\n",
    "    if J0 is None:\n",
    "        Jk_1 = get_jacobs_from_cams(cams,end_point,robot,q0.T,rot)\n",
    "    else:\n",
    "        Jk_1 = J0.copy()\n",
    "    fk_1 = fk.copy()\n",
    "    qk = q0.copy()\n",
    "    qk_1 = q0.copy()\n",
    "    count = 0\n",
    "    max_iter = 1e3\n",
    "    nm = fk.T*fk\n",
    "    while nm > err:\n",
    "        count += 1\n",
    "        if count > max_iter: break\n",
    "        delta_q = qk - qk_1\n",
    "        delta_f = fk - fk_1\n",
    "        if idea:\n",
    "            Jk = get_jacobs_from_cams(cams,end_point,robot,qk,rot)\n",
    "        else:\n",
    "            Jk = get_Jk(Jk_1,delta_q,delta_f)\n",
    "        \n",
    "        if use_pinv:\n",
    "            next_q = be_in_good(qk - pinv(Jk)*fk)\n",
    "        else:\n",
    "            next_q = get_next_q(Jk,Jk_1,qk,fk,delta_q)\n",
    "        \n",
    "        new_end_point = get_end_point(cams,robot,next_q)\n",
    "        plot_end_point_line(cams,end_point,new_end_point)\n",
    "        next_f = new_end_point - img_point\n",
    "        fk_1 = fk\n",
    "        qk_1 = qk\n",
    "        qk = next_q\n",
    "        fk = next_f\n",
    "        end_point = new_end_point\n",
    "        if debug:\n",
    "            print count, fk\n",
    "            if nm < fk.T*fk:\n",
    "                print \"发散\"\n",
    "            else:\n",
    "                print \"收敛\"\n",
    "        nm = fk.T*fk\n",
    "    return qk,Jk,count,nm"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def test_move_to(cams,robot,q0,img_point,J0=None):\n",
    "    print \"不考虑输入的J0:\"\n",
    "    for rot in [True,False]:\n",
    "        for idea in [True,False]:\n",
    "            for use_pinv in [True,False]:\n",
    "                string = \"\"\n",
    "                if rot:\n",
    "                    string+=\"考虑转动，\"\n",
    "                else:\n",
    "                    string+=\"不考虑转动，\"\n",
    "                if idea:\n",
    "                    string+=\"使用理想雅克比，\"\n",
    "                else:\n",
    "                    string+=\"使用估计雅克比，\"\n",
    "                if use_pinv:\n",
    "                    string+=\"使用伪逆求解，\"\n",
    "                else:\n",
    "                    string+=\"不使用伪逆求解，\"\n",
    "                qk,Jk,count,nm = move_to(cams,robot,q0,img_point,rot=rot,idea=idea,use_pinv=use_pinv)\n",
    "                string+= \"迭代次数： %f, \" % count\n",
    "                string+= \"最终误差： %f, \" % nm[0,0]\n",
    "                print string\n",
    "    print \"使用输入的J0\"\n",
    "    for use_pinv in [True,False]:\n",
    "        string = \"\"\n",
    "        if use_pinv:\n",
    "            string+=\"使用伪逆求解，\"\n",
    "        else:\n",
    "            string+=\"不使用伪逆求解，\"\n",
    "        qk,Jk,count,nm = move_to(cams,nao.LeftHand,q0,img_point,J0=J0,use_pinv=use_pinv)\n",
    "        string+= \"迭代次数： %d, \" % count\n",
    "        string+= \"最终误差： %f, \" % nm[0,0]\n",
    "        print string"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# 用最小二乘估计出初始的 J0\n",
    "def get_J0(cams,robot,q0):\n",
    "    Dp = None\n",
    "    Dq = None\n",
    "    n = len(q0) # 关节空间维度\n",
    "    m = len(cams)*2 # 图像空间维度\n",
    "    # 共有 m x n 个未知数，\n",
    "    # 每一对 dp, dq 可以提供 m 个方程\n",
    "    # 因此至少需要 n 组对应值\n",
    "    # 为实现最小二乘，现在取 2xn 组值\n",
    "    q = q0.copy()\n",
    "    p = get_end_point(cams,robot,q)\n",
    "    for i in range(n*3):\n",
    "        dq = numpy.random.random(q0.shape)*0.2\n",
    "        dp = get_end_point(cams,robot,q+dq) - p\n",
    "        if Dp is None:\n",
    "            Dp = dp\n",
    "            Dq = dq\n",
    "        else:\n",
    "            Dp = np.hstack((Dp,dp))\n",
    "            Dq = np.hstack((Dq,dq))\n",
    "    J0 = Dp*pinv(Dq)\n",
    "    return J0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from mdh_nao import Nao\n",
    "nao = Nao(\"nao\")\n",
    "cam = Cam(\"one image\")\n",
    "cam.Tr = rpy2tr(0.01,0.02,0.03)*transl(-220,-200,500)\n",
    "q0 = mat([0.1,0.2,0.3,0.4,0.5]).T\n",
    "p2d0 = cam.plot_robot_point(nao.LeftHand,q0.T)\n",
    "img_point = p2d0.copy()\n",
    "img_point[0,0] += 6\n",
    "img_point[1,0] += 6\n",
    "cam.plot_img_point(img_point,(0,0,0))\n",
    "print p2d0,img_point\n",
    "show_cams([cam])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 估计初始雅克比"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "clear_cams([cam])\n",
    "J0 = get_J0([cam],nao.LeftHand,q0)\n",
    "show_cams([cam])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "J0 = get_J0([cam],nao.LeftHand,q0)\n",
    "test_move_to([cam],nao.LeftHand,q0,img_point,J0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "clear_cams([cam])\n",
    "print move_to([cam],nao.LeftHand,q0,img_point,rot=True,use_pinv=False) # 可解得\n",
    "show_cams([cam])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 多个相机图像"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 两个相机图像\n",
    "\n",
    "机械臂有5个自由度, 两个相机限定 4 个自由度, 差别较大时也可解,但迭代次数过多,考虑到迭代精度,实际意义也不大"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from mdh_nao import Nao\n",
    "nao = Nao(\"nao\")\n",
    "cam0 = Cam(\"cam one\")\n",
    "cam0.Tr = rpy2tr(0.01,0.02,0.03)*transl(-220,-200,600)\n",
    "cam1 = Cam(\"cam two\")\n",
    "cam1.Tr = rpy2tr(0.01,0.02,0.03)*transl(-150,-150,600)\n",
    "q0 = mat([0.1,0.2,0.3,0.4,0.5]).T\n",
    "delta_q = mat([0.1,0.2,0.3,0.2,0.5]).T\n",
    "q1 = q0+delta_q\n",
    "end_point = get_end_point([cam0,cam1],nao.LeftHand,q0)\n",
    "img_point = get_end_point([cam0,cam1],nao.LeftHand,q1,(0,0,0))\n",
    "print end_point,img_point,end_point-img_point\n",
    "show_cams([cam0,cam1])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "J0 = get_J0([cam0,cam1],nao.LeftHand,q0)\n",
    "test_move_to([cam0,cam1],nao.LeftHand,q0,img_point,J0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 不考虑转动，使用理想雅克比，使用伪逆求解，迭代次数\n",
    "clear_cams([cam0,cam1])\n",
    "print move_to([cam0,cam1],nao.LeftHand,q0,img_point,rot=False,idea=True,use_pinv=True) # 可解得\n",
    "show_cams([cam0,cam1])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 使用估计的J0 伪逆求解\n",
    "clear_cams([cam0,cam1])\n",
    "move_to([cam0,cam1],nao.LeftHand,q0,img_point,J0=J0,use_pinv=True) # 可解得\n",
    "show_cams([cam0,cam1])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 使用估计的J0 不使用伪逆\n",
    "clear_cams([cam0,cam1])\n",
    "move_to([cam0,cam1],nao.LeftHand,q0,img_point,J0=J0,use_pinv=False) # 可解得\n",
    "show_cams([cam0,cam1])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "clear_cams([cam0,cam1])\n",
    "J0 = get_J0([cam0,cam1],nao.LeftHand,q0)\n",
    "show_cams([cam0,cam1])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "若去掉机械臂的一个自由度:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from mdh_nao import Nao\n",
    "nao = Nao(\"nao\")\n",
    "nao.LeftHand.lock(0.5)\n",
    "cam0 = Cam(\"cam one remove a dof\")\n",
    "cam0.Tr = rpy2tr(0.01,0.02,0.03)*transl(-220,-200,600)\n",
    "cam1 = Cam(\"cam two remove a dof\")\n",
    "cam1.Tr = rpy2tr(0.01,0.02,0.03)*transl(-150,-150,600)\n",
    "q0 = mat([0.1,0.2,0.3,0.4]).T\n",
    "delta_q = mat([0.01,0.2,0.3,0.04]).T\n",
    "q1 = q0+delta_q\n",
    "end_point = get_end_point([cam0,cam1],nao.LeftHand,q0)\n",
    "img_point = get_end_point([cam0,cam1],nao.LeftHand,q1,(0,0,0))\n",
    "print end_point,img_point,end_point-img_point\n",
    "show_cams([cam0,cam1])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "J0 = get_J0([cam0,cam1],nao.LeftHand,q0)\n",
    "test_move_to([cam0,cam1],nao.LeftHand,q0,img_point,get_J0([cam0,cam1],nao.LeftHand,q0))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 不考虑转动，估计的雅克比，使用伪逆求解\n",
    "clear_cams([cam0,cam1])\n",
    "print move_to([cam0,cam1],nao.LeftHand,q0,img_point,rot=False,use_pinv=True)\n",
    "show_cams([cam0,cam1])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 不考虑转动，估计的雅克比，不使用伪逆求解\n",
    "clear_cams([cam0,cam1])\n",
    "print move_to([cam0,cam1],nao.LeftHand,q0,img_point,rot=False,use_pinv=False,err=1.0)\n",
    "show_cams([cam0,cam1])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 使用输入的J0, 伪逆求解\n",
    "clear_cams([cam0,cam1])\n",
    "print move_to([cam0,cam1],nao.LeftHand,q0,img_point,J0=J0,use_pinv=True)\n",
    "show_cams([cam0,cam1])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 使用输入的J0, 非伪逆求解\n",
    "clear_cams([cam0,cam1])\n",
    "print move_to([cam0,cam1],nao.LeftHand,q0,img_point,J0=J0,use_pinv=True)\n",
    "show_cams([cam0,cam1])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 三个相机图像\n",
    "\n",
    "机械臂只有 5个 自由度"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from mdh_nao import Nao\n",
    "nao = Nao(\"nao\")\n",
    "cam0 = Cam(\"cam 0\")\n",
    "cam0.Tr = rpy2tr(0.01,0.02,0.03)*transl(-220,-200,600)\n",
    "cam1 = Cam(\"cam 1\")\n",
    "cam1.Tr = rpy2tr(0.01,0.02,0.03)*transl(-150,-150,600)\n",
    "cam2 = Cam(\"cam 2\")\n",
    "cam2.Tr = rpy2tr(0.01,0.02,0.03)*transl(-190,-150,600)\n",
    "q0 = mat([0.1,0.2,0.3,0.4,0.5]).T\n",
    "delta_q = mat([0.01,0.2,0.3,0.04,0.05]).T\n",
    "q1 = q0+delta_q\n",
    "end_point = get_end_point([cam0,cam1,cam2],nao.LeftHand,q0)\n",
    "img_point = get_end_point([cam0,cam1,cam2],nao.LeftHand,q1,(0,0,0))\n",
    "print end_point,img_point,end_point-img_point\n",
    "show_cams([cam0,cam1,cam2])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "J0 = get_J0([cam0,cam1,cam2],nao.LeftHand,q0)\n",
    "test_move_to([cam0,cam1,cam2],nao.LeftHand,q0,img_point,J0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 不考虑转动，使用估计雅克比，使用伪逆求解\n",
    "clear_cams([cam0,cam1,cam2])\n",
    "print move_to([cam0,cam1,cam2],nao.LeftHand,q0,img_point,rot=False,use_pinv=True)\n",
    "show_cams([cam0,cam1,cam2])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 不考虑转动，使用估计雅克比，使用伪逆求解\n",
    "clear_cams([cam0,cam1,cam2])\n",
    "print move_to([cam0,cam1,cam2],nao.LeftHand,q0,img_point,J0=J0,use_pinv=True)\n",
    "show_cams([cam0,cam1,cam2])"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
